/* -*-c++-*- IfcGears - www.ifcgears.com  - Copyright (C) 2011 Fabian Gerold
 *
 * This library is open source and may be redistributed and/or modified under  
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or 
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 * 
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 
 * OpenSceneGraph Public License for more details.
*/

#include <osg/Geode>
#include <osg/Geometry>
#include <osg/Group>
#include <osg/MatrixTransform>
#include <osgUtil/Tessellator>
#include <osgText/Text>

#include "carve/carve.hpp"
#include "carve/csg.hpp"
#include "carve/csg_triangulator.hpp"
#include "carve/faceloop.hpp"
#include "carve/geom3d.hpp"
#include "carve/input.hpp"
#include "carve/polyhedron_base.hpp"
#include "carve/poly.hpp"
#include "carve/poly.hpp"
#include "carve/triangulator.hpp"
#include "carve/common/geometry.hpp"

#include "ifcgears/model/IfcGearsException.h"
#include "Utility.h"
#include "Carve2OsgConverter.h"

void Carve2OsgConverter::drawFace( const carve::mesh::Face<3>* face, osg::Geode* geode )
{
	std::vector<carve::geom3d::Vector> v;
	v.resize(face->nVertices());
	carve::mesh::Edge<3> *e = face->edge;
	const size_t num_vertices = face->nVertices();
	for( size_t i = 0; i < num_vertices; ++i )
	{
		v[i] = e->v1()->v;
		e = e->next;
	}
	carve::geom::vector<3> normal = face->plane.N;

	osg::Vec3Array* vertices = new osg::Vec3Array();
	osg::Vec3Array* normals = new osg::Vec3Array();
	normals->push_back(osg::Vec3f(normal.x, normal.y, normal.z));

	if( num_vertices < 4 )
	{
		throw IfcGearsException( "BooleanModeller::drawFace is meant only for num vertices > 4" );
	}

	carve::geom3d::Vector* vec;

	for( size_t i = 0; i < num_vertices; ++i )
	{
		vec = &v[num_vertices-i-1];
		vertices->push_back(osg::Vec3f(vec->x, vec->y, vec->z));
	}

	osg::Geometry* geometry = new osg::Geometry();
	geometry->setVertexArray( vertices );
	geometry->setNormalArray( normals );

	geometry->setNormalBinding( osg::Geometry::BIND_OVERALL );
	geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON,0,vertices->size()));
	geode->addDrawable(geometry);

	osg::ref_ptr<osgUtil::Tessellator> m_tesselator = new osgUtil::Tessellator();
	m_tesselator->setTessellationType(osgUtil::Tessellator::TESS_TYPE_POLYGONS);
	m_tesselator->setWindingType( osgUtil::Tessellator::TESS_WINDING_ODD );
	m_tesselator->setBoundaryOnly(true);
	m_tesselator->retessellatePolygons(*geometry);
}

void Carve2OsgConverter::drawMesh( const carve::mesh::Mesh<3>* mesh, osg::Geode* geode )
{
	osg::ref_ptr<osg::Vec3Array> vertices_tri = new osg::Vec3Array();
	osg::ref_ptr<osg::Vec3Array> normals_tri = new osg::Vec3Array();
	osg::ref_ptr<osg::Vec4Array> colors_tri = new osg::Vec4Array();

	osg::ref_ptr<osg::Vec3Array> vertices_quad = new osg::Vec3Array();
	osg::ref_ptr<osg::Vec3Array> normals_quad = new osg::Vec3Array();
	osg::ref_ptr<osg::Vec4Array> colors_quad = new osg::Vec4Array();

	std::vector<const carve::mesh::Vertex<3> *> v;
	const carve::geom::vector<3>* v0;
	const carve::geom::vector<3>* v1;
	const carve::geom::vector<3>* v2;
	const carve::geom::vector<3>* v3;

	osg::Vec4f gray( 0.6f, 0.6f, 0.6f, 0.f );

	const size_t num_faces = mesh->faces.size();
	for( size_t i = 0; i != num_faces; ++i )
	{
		carve::mesh::Face<3>* f = mesh->faces[i];
		if(f->nVertices() > 4)
		{
			drawFace(f, geode );
			continue;
		}
		osg::Vec3 normal(f->plane.N.v[0],f->plane.N.v[1],f->plane.N.v[2]);
		f->getVertices(v);

		if( f->nVertices() == 3 )
		{
			v0 = &(v[0]->v);
			v1 = &(v[1]->v);
			v2 = &(v[2]->v);
			normals_tri->push_back( normal );
			normals_tri->push_back( normal );
			normals_tri->push_back( normal );
			colors_tri->push_back( gray );
			colors_tri->push_back( gray );
			colors_tri->push_back( gray );

			vertices_tri->push_back( osg::Vec3(v0->x, v0->y, v0->z ) );
			vertices_tri->push_back( osg::Vec3(v1->x, v1->y, v1->z ) );
			vertices_tri->push_back( osg::Vec3(v2->x, v2->y, v2->z ) );
		}
		else if( f->nVertices() == 4 )
		{
			v0 = &(v[0]->v);
			v1 = &(v[1]->v);
			v2 = &(v[2]->v);
			v3 = &(v[3]->v);

			normals_quad->push_back( normal );
			normals_quad->push_back( normal );
			normals_quad->push_back( normal );
			normals_quad->push_back( normal );

			colors_quad->push_back( gray );
			colors_quad->push_back( gray );
			colors_quad->push_back( gray );
			colors_quad->push_back( gray );

			vertices_quad->push_back( osg::Vec3(v0->x, v0->y, v0->z ) );
			vertices_quad->push_back( osg::Vec3(v1->x, v1->y, v1->z ) );
			vertices_quad->push_back( osg::Vec3(v2->x, v2->y, v2->z ) );
			vertices_quad->push_back( osg::Vec3(v3->x, v3->y, v3->z ) );
		}
	}

	if( vertices_tri->size() > 0 )
	{
		osg::Geometry* geometry = new osg::Geometry();
		geometry->setVertexArray( vertices_tri );
		
		geometry->setNormalArray( normals_tri );
		geometry->setNormalBinding( osg::Geometry::BIND_PER_VERTEX );

		geometry->setColorArray( colors_tri );
		geometry->setColorBinding( osg::Geometry::BIND_PER_VERTEX );

		geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::TRIANGLES,0,vertices_tri->size()));
		geode->addDrawable( geometry );
	}

	if( vertices_quad->size() > 0 )
	{
		osg::Geometry* geometry = new osg::Geometry();
		geometry->setVertexArray( vertices_quad );

		geometry->setNormalArray( normals_quad );
		geometry->setNormalBinding( osg::Geometry::BIND_PER_VERTEX );
		
		geometry->setColorArray( colors_quad );
		geometry->setColorBinding( osg::Geometry::BIND_PER_VERTEX );

		geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::QUADS,0,vertices_quad->size()));
		geode->addDrawable( geometry );
	}
}

void Carve2OsgConverter::drawPolyhedron( const shared_ptr<carve::poly::Polyhedron>& polyhedron, osg::Geode* geode )
{
	shared_ptr<carve::mesh::MeshSet<3> > mesh_set( carve::meshFromPolyhedron(polyhedron.get(), -1) );
	for( size_t i = 0; i < mesh_set->meshes.size(); ++i )
	{
		drawMesh(mesh_set->meshes[i], geode );
	}
}

void Carve2OsgConverter::drawOpenMesh(	const shared_ptr<carve::input::PolyhedronData>& poly_data, osg::Geode* geode )
{
	const std::vector<carve::geom3d::Vector>& vec_points = poly_data->points;
	const std::vector<int>& face_indices = poly_data->faceIndices;

	osg::ref_ptr<osg::Vec3Array> vertices = new osg::Vec3Array();
	osg::ref_ptr<osg::Vec3Array> normals = new osg::Vec3Array();
	osg::ref_ptr<osg::DrawElementsUInt> triangles = new osg::DrawElementsUInt( osg::PrimitiveSet::TRIANGLES, 0 );

	osg::ref_ptr<osg::Vec3Array> normals_quad = new osg::Vec3Array();
	osg::ref_ptr<osg::DrawElementsUInt> quads = new osg::DrawElementsUInt( osg::PrimitiveSet::QUADS, 0 );

	std::vector<carve::geom3d::Vector>::const_iterator it_points;
	for( it_points = vec_points.begin(); it_points != vec_points.end(); ++it_points )
	{
		const carve::geom3d::Vector& vec = (*it_points);
		vertices->push_back( osg::Vec3f( vec.x, vec.y, vec.z ) );
	}

	std::vector<int>::const_iterator it_face_indices;
	for( it_face_indices = face_indices.begin(); it_face_indices != face_indices.end(); ++it_face_indices )
	{
		int num_indices = (*it_face_indices);
		if( num_indices == 3 )
		{
			++it_face_indices;
			int vi0 = (*it_face_indices);
			triangles->push_back( vi0 );
			osg::Vec3f& v0 = vertices->at( vi0 );

			++it_face_indices;
			int vi1 = (*it_face_indices);
			triangles->push_back( vi1 );
			osg::Vec3f& v1 = vertices->at( vi1 );

			++it_face_indices;
			int vi2 = (*it_face_indices);
			triangles->push_back( vi2 );
			osg::Vec3f& v2 = vertices->at( vi2 );

			// TODO: get edged from connectivity and compute intermediate normal if angle between faces is small
			osg::Vec3f normal( (v1-v0)^(v2-v0) );
			normal.normalize();
			normals->push_back(normal);
			
		}
		else if( num_indices == 4 )
		{
			++it_face_indices;
			int vi0 = (*it_face_indices);
			quads->push_back( vi0 );
			osg::Vec3f& v0 = vertices->at( vi0 );

			++it_face_indices;
			int vi1 = (*it_face_indices);
			quads->push_back( vi1 );
			osg::Vec3f& v1 = vertices->at( vi1 );

			++it_face_indices;
			int vi2 = (*it_face_indices);
			quads->push_back( vi2 );
			osg::Vec3f& v2 = vertices->at( vi2 );

			++it_face_indices;
			int vi3 = (*it_face_indices);
			quads->push_back( vi3 );
			
			osg::Vec3f normal( (v1-v0)^(v2-v0) );
			normal.normalize();
			normals->push_back(normal);

		}
		else
		{
			for( int i=0; i<num_indices; ++i )
			{
				(++it_face_indices);

			}
		}
	}

	osg::Geometry* geometry = new osg::Geometry();
	geometry->setVertexArray( vertices );

	geometry->setNormalArray( normals );
	geometry->setNormalBinding( osg::Geometry::BIND_PER_VERTEX );

	if( triangles->size() > 0 )
	{
		geometry->addPrimitiveSet( triangles );
	}
	if( quads->size() > 0 )
	{
		geometry->addPrimitiveSet( quads );
	}
	geode->addDrawable( geometry );
}

void Carve2OsgConverter::drawPolyline( const shared_ptr<carve::input::PolylineSetData>& polyline_data, osg::Geode* geode )
{
	osg::Vec3Array* vertices = new osg::Vec3Array();
	carve::line::PolylineSet* polyline_set = polyline_data->create();

	carve::line::PolylineSet::const_line_iter it;
	for( it = polyline_set->lines.begin(); it != polyline_set->lines.end(); ++it )
	{
		carve::line::Polyline* pline = *it;
		size_t vertex_count = pline->vertexCount();

		for( size_t vertex_i = 0; vertex_i < vertex_count; ++vertex_i )
		{
			const carve::line::Vertex* v = pline->vertex( vertex_i );
			vertices->push_back( osg::Vec3d( v->v[0], v->v[1], v->v[2] ) );

		}
	}

	osg::Geometry* geometry = new osg::Geometry();
	geometry->setVertexArray( vertices );
	geometry->addPrimitiveSet( new osg::DrawArrays(osg::PrimitiveSet::LINE_STRIP, 0, vertices->size()) );

	geode->addDrawable(geometry);

}

bool Carve2OsgConverter::checkPolyHedron( shared_ptr<carve::poly::Polyhedron>& poly, std::stringstream& err_poly, int entity_id )
{
	// check opening polyhedron
	if( !poly )
	{
		err_poly << "Polyhedron of entity #" << entity_id << " not valid";
		return false;
	}

	std::stringstream err;
	if( poly->manifoldCount() == 0 )
	{
		err << "manifoldCount() == 0";
	}

	if( poly->vertices.size() < 3 )
	{
		err << "vertices.size() < 3";
	}

	if( poly->hasOpenManifolds() )
	{
		err << "hasOpenManifolds";
	}

	for( int mani=0; mani<poly->manifoldCount(); ++mani )
	{
		if( !poly->manifold_is_closed[mani] )
		{
			err << "!manifold_is_closed[" << mani << "]";
		}
		if( poly->manifold_is_negative[mani] )
		{
			err << "!manifold_is_negative[" << mani << "]";
		}
	}
	for( int i=0; i<poly->faces.size(); ++i )
	{
		//poly->faces[i].
	}
	//faceRecalc

	if( err.tellp() > 0 )
	{
		err_poly << "Polyhedron of entity #" << entity_id << " has problems:";
		err_poly << err.str().c_str();
		return false;
	}
	return true;
}

void Carve2OsgConverter::convertOsgGroup( const osg::Group* src, carve::input::PolyhedronData& target )
{
	int num_children = src->getNumChildren();
	for( int i=0; i<num_children; ++i )
	{
		const osg::Node* node = src->getChild(i);
		const osg::Group* child_group = dynamic_cast<const osg::Group*>(node);
		if( child_group )
		{
			convertOsgGroup( child_group, target );
			continue;
		}
		const osg::Geode* child_geode = dynamic_cast<const osg::Geode*>(node);
		if( child_geode )
		{
			const osg::Geode::DrawableList& drawable_list = child_geode->getDrawableList();
			osg::Geode::DrawableList::const_iterator it_drawables;
			for( it_drawables=drawable_list.begin(); it_drawables!=drawable_list.end(); ++it_drawables )
			{
				osg::Drawable* drawable = (*it_drawables);
				const osg::Geometry* child_gemetry = dynamic_cast<const osg::Geometry*>(drawable);
				if( child_gemetry )
				{
					const osg::Array* vertices_array = child_gemetry->getVertexArray();
					const osg::Vec3dArray* vertices_d = dynamic_cast<const osg::Vec3dArray*>(vertices_array);
					
					const int vertex_offset = target.getVertexCount();

					if( vertices_d )
					{
						//std::vector<carve::poly::Vertex<3> >& target_vertices = target->vertices;
						const int num_vertices = vertices_d->size();
						for( int i_vertex=0; i_vertex<num_vertices; ++i_vertex )
						{
							const osg::Vec3d& vertex = vertices_d->at(i_vertex);
							target.addVertex(carve::geom::VECTOR(vertex.x(), vertex.y(), vertex.z() ) );

						}
					}
					else
					{
						const osg::Vec3Array* vertices_float = dynamic_cast<const osg::Vec3Array*>(vertices_array);
						if( vertices_float )
						{
							const int num_vertices = vertices_float->size();
							for( int i_vertex=0; i_vertex<num_vertices; ++i_vertex )
							{
								const osg::Vec3& vertex = vertices_float->at(i_vertex);
								target.addVertex(carve::geom::VECTOR(vertex.x(), vertex.y(), vertex.z() ) );
							}
						}
						else
						{
							continue;
						}
					}

					const osg::Geometry::PrimitiveSetList& primitive_sets = child_gemetry->getPrimitiveSetList();
					osg::Geometry::PrimitiveSetList::const_iterator it_primitives;
					for( it_primitives=primitive_sets.begin(); it_primitives!=primitive_sets.end(); ++it_primitives )
					{
						const osg::PrimitiveSet* p_set = (*it_primitives);

						const osg::DrawElementsUInt* elements = dynamic_cast<const osg::DrawElementsUInt*>(p_set);
						if( elements )
						{
							if( elements->getMode() == osg::PrimitiveSet::QUADS )
							{

								const osg::DrawElementsUInt& ele_ref = (*elements);
								const int num_elements = elements->size();
								if( elements->size() > 3 )
								{
									for( int k=0; k<num_elements-3; k+=4 )
									{
										int p0 = ele_ref[k] + vertex_offset;
										int p1 = ele_ref[k+1] + vertex_offset;
										int p2 = ele_ref[k+2] + vertex_offset;
										int p3 = ele_ref[k+3] + vertex_offset;

										carve::geom3d::Vector v0 = target.getVertex(p0);
										carve::geom3d::Vector v1 = target.getVertex(p1);
										carve::geom3d::Vector v2 = target.getVertex(p2);
										carve::geom3d::Vector v3 = target.getVertex(p3);
										osg::Vec3 pt0(v0.v[0],v0.v[1],v0.v[2]);
										osg::Vec3 pt1(v1.v[0],v1.v[1],v1.v[2]);
										osg::Vec3 pt2(v2.v[0],v2.v[1],v2.v[2]);
										osg::Vec3 pt3(v3.v[0],v3.v[1],v3.v[2]);

										if( (pt0 -pt1).length2() < 0.00001 )
										{
											continue;
										}
										if( (pt1 -pt2).length2() < 0.00001 )
										{
											continue;
										}
										if( (pt2 -pt3).length2() < 0.00001 )
										{
											continue;
										}
										if( (pt3 -pt0).length2() < 0.00001 )
										{
											continue;
										}
										std::vector<int> face_verts;
										face_verts.push_back(p0);
										face_verts.push_back(p1);
										face_verts.push_back(p2);
										face_verts.push_back(p3);
										target.addFace(face_verts.begin(), face_verts.end());

									}
								}
							

							}
							else if( elements->getMode() == osg::PrimitiveSet::TRIANGLES )
							{
								const osg::DrawElementsUInt& ele_ref = (*elements);
								const int num_elements = elements->size();
								if( elements->size() > 3 )
								{
									for( int k=0; k<num_elements-2; k+=3 )
									{
										int p0 = ele_ref[k] + vertex_offset;
										int p1 = ele_ref[k+1] + vertex_offset;
										int p2 = ele_ref[k+2] + vertex_offset;
										
										std::vector<int> face_verts;
										face_verts.push_back(p0);
										face_verts.push_back(p1);
										face_verts.push_back(p2);
										target.addFace(face_verts.begin(), face_verts.end());

										carve::geom3d::Vector v0 = target.getVertex(p0);
										carve::geom3d::Vector v1 = target.getVertex(p1);
										carve::geom3d::Vector v2 = target.getVertex(p2);
										osg::Vec3 pt0(v0.v[0],v0.v[1],v0.v[2]);
										osg::Vec3 pt1(v1.v[0],v1.v[1],v1.v[2]);
										osg::Vec3 pt2(v2.v[0],v2.v[1],v2.v[2]);
										
										if( (pt0 -pt1).length2() < 0.00001 )
										{
											continue;
										}
										if( (pt1 -pt2).length2() < 0.00001 )
										{
											continue;
										}
										if( (pt2 -pt0).length2() < 0.00001 )
										{
											continue;
										}
									}
								}

							}
							else if( elements->getMode() == osg::PrimitiveSet::TRIANGLE_STRIP )
							{
								const osg::DrawElementsUInt& ele_ref = (*elements);
								const int num_elements = elements->size();
								if( elements->size() > 3 )
								{
									for( int k=0; k<num_elements-3; k+=2 )
									{
										int p0 = ele_ref[k] + vertex_offset;
										int p1 = ele_ref[k+1] + vertex_offset;
										int p2 = ele_ref[k+2] + vertex_offset;
										int p3 = ele_ref[k+3] + vertex_offset;
										
										carve::geom3d::Vector v0 = target.getVertex(p0);
										carve::geom3d::Vector v1 = target.getVertex(p1);
										carve::geom3d::Vector v2 = target.getVertex(p2);
										carve::geom3d::Vector v3 = target.getVertex(p3);
										osg::Vec3 pt0(v0.v[0],v0.v[1],v0.v[2]);
										osg::Vec3 pt1(v1.v[0],v1.v[1],v1.v[2]);
										osg::Vec3 pt2(v2.v[0],v2.v[1],v2.v[2]);
										osg::Vec3 pt3(v3.v[0],v3.v[1],v3.v[2]);

										if( (pt0 -pt1).length2() < 0.00001 )
										{
											continue;
										}
										if( (pt1 -pt2).length2() < 0.00001 )
										{
											continue;
										}
										if( (pt2 -pt3).length2() < 0.00001 )
										{
											continue;
										}
										if( (pt3 -pt0).length2() < 0.00001 )
										{
											continue;
										}
										std::vector<int> face_verts;
										face_verts.push_back(p0);
										face_verts.push_back(p1);
										face_verts.push_back(p2);
										target.addFace(face_verts.begin(), face_verts.end());


										std::vector<int> face_verts1;
										face_verts1.push_back(p1);
										face_verts1.push_back(p3);
										face_verts1.push_back(p2);
										target.addFace(face_verts1.begin(), face_verts1.end());
									}
								}
							}
						}
					}
				}
			}
		}
	}
}
